{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  # We must install required packages if we are in Google Colab\n",
    "  import google.colab\n",
    "  %pip install roboticstoolbox-python>=1.0.2\n",
    "except:\n",
    "  # We are not in Google Colab\n",
    "  # Apply custon style to notebook\n",
    "  from IPython.core.display import HTML\n",
    "  import pathlib\n",
    "  styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "  styles = open(styles_path, \"r\").read()\n",
    "  HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 1.0 The Manipulator Hessian\n",
    "\n",
    "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part II: Acceleration and Advanced Applications}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "<br>\n",
    "\n",
    "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n",
    "It is beneficial to read these sections of the paper before attempting the notebook Section.\n",
    "\n",
    "### Contents\n",
    "\n",
    "[1.1 Prelude](#pre)\n",
    "* Part I of the Tutorial\n",
    "  * Deriving the Manipulator Jacobian\n",
    "  * First Derivative of an Elementary Transform\n",
    "  * The Manipulator Jacobian\n",
    "  * Fast Manipulator Jacobian\n",
    "\n",
    "[1.2 The Manipulator Hessian](#hess)\n",
    "* The Manipulator Hessian\n",
    "\n",
    "[1.3 Fast Manipulator Hessian](#fhess)\n",
    "* Fast Manipulator Hessian\n",
    "\n",
    "[1.4 Changing Hessian Frame](#frame)\n",
    "\n",
    "[1.5 Robotics Toolbox Jacobians](#rtb)\n",
    "\n",
    "[1.6 Speed Analysis](#fast)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb\n",
    "\n",
    "# spatial math provides objects for representing transformations\n",
    "import spatialmath as sm\n",
    "import spatialmath.base as smb\n",
    "\n",
    "# use timeit to benchmark some methods\n",
    "from timeit import default_timer as timer\n",
    "\n",
    "# ansitable is a great package for printing tables in a terminal\n",
    "from ansitable import ANSITable"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='pre'></a>\n",
    "### 1.1 Prelude\n",
    "---\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We begin with the forward kineamtics of a manipulator as described in Part I\n",
    "\n",
    "\\begin{align*}\n",
    "    {^0\\bf{T}_e(t)}  &= {\\cal K}(\\bf{q}(t)) \\\\\n",
    "    &= \\prod_{i=1}^{M} \\bf{E}_i(\\eta_i).\n",
    " \\end{align*}\n",
    "\n",
    "where $\\bf{q}(t) \\in \\mathbb{R}^n$ is the vector of joint generalised coordinates, $n$ is the number of joints, and $M$ is the number of elementary transforms $\\bf{E}_i \\in \\bf{SE}(3)$. From the derivative of this we can express the spatial translational and angular velocity as a function of the joint coordinates and velocities\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{\\nu} =\n",
    "    \\begin{pmatrix}\n",
    "        \\bf{v} \\\\ \\bf{\\omega}\n",
    "    \\end{pmatrix}\n",
    "    &=\n",
    "    \\bf{J}(\\bf{q})\\dot{\\bf{q}}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{J}(\\bf{q})$ is the manipulator Jacobian, $\\bf{v} = (v_x, \\ v_y, \\ v_z)$, and $\\bf{\\omega} =(\\omega_x, \\ \\omega_y, \\ \\omega_z)$. Taking the temporal derivative gives\n",
    "\n",
    "\\begin{align*}\n",
    "    \\begin{pmatrix}\n",
    "        \\dot{\\bf{v}} \\\\ \\dot{\\bf{\\omega}}\n",
    "    \\end{pmatrix}\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\bf{a} \\\\ \\bf{\\alpha}\n",
    "    \\end{pmatrix}\n",
    "    =\n",
    "    \\dot{\\bf{J}} \\dot{\\bf{q}} + \\bf{J} \\ddot{\\bf{q}}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{a} \\in \\mathbb{R}^3$ is the end-effector translational acceleration, $\\bf{\\alpha} \\in \\mathbb{R}^3$ is the end-effector angular acceleration, and\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dot{\\bf{J}}\n",
    "    &=\n",
    "    \\dfrac{\\mathrm{d} \\bf{J}(\\bf{q})}\n",
    "        {\\mathrm{d} t}  \\\\\n",
    "    &=\n",
    "    \\dfrac{\\partial \\bf{J}(\\bf{q})}\n",
    "        {\\partial q_1} \\dot{q}_1\n",
    "    +\n",
    "    \\dfrac{\\partial \\bf{J}(\\bf{q})}\n",
    "        {\\partial q_2} \\dot{q}_2\n",
    "    + \\cdots +\n",
    "    \\dfrac{\\partial \\bf{J}}\n",
    "        {\\partial q_n} \\dot{q}_n \\\\\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\dfrac{\\partial \\bf{J}(\\bf{q})}\n",
    "            {\\partial q_1} &\n",
    "        \\dfrac{\\partial \\bf{J}(\\bf{q})}\n",
    "            {\\partial q_2} &\n",
    "        \\cdots &\n",
    "        \\dfrac{\\partial \\bf{J}(\\bf{q})}\n",
    "            {\\partial q_n}\n",
    "    \\end{pmatrix}\n",
    "    \\dot{\\bf{q}} \\\\\n",
    "    &=\n",
    "    \\bf{H}(\\bf{q}) \\dot{\\bf{q}} \\\\\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\bf{H}_a(\\bf{q}) \\\\ \\bf{H}_\\alpha(\\bf{q})\n",
    "    \\end{pmatrix} \\dot{\\bf{q}}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{H}(\\bf{q}) \\in \\mathbb{R}^{6 \\times n \\times n}$ in the manipulator Hessian tensor, which is the partial derivative of the manipulator Jacobian with respect to the joint coordinates, $\\bf{H}_a(\\bf{q}) \\in \\mathbb{R}^{3 \\times n \\times n}$ forms the translational component of the Hessian, and $\\bf{H}_\\alpha(\\bf{q}) \\in \\mathbb{R}^{3 \\times n \\times n}$ forms the angular component of the Hessian."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From Part I of the Tutorial, we the derivative of an elementary transform with respect to a joint coordinate is obtained using one of\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{R_{x}}(\\theta)}\n",
    "        {\\mathrm{d} \\theta}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & -1 & 0 \\\\\n",
    "        0 & 1 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix} \\bf{T}_{R_{x}}(\\theta),\\\\\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{R_{y}}(\\theta)}\n",
    "        {\\mathrm{d} \\theta}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 1 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        -1 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix} \\bf{T}_{R_{y}}(\\theta), \\\\\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{R_{z}}(\\theta)}\n",
    "        {\\mathrm{d} \\theta}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & -1 & 0 & 0 \\\\\n",
    "        1 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0  \n",
    "    \\end{pmatrix} \\bf{T}_{R_{z}}(\\theta),\n",
    "\\end{align*}\n",
    "\n",
    "for a revolute joint, or one of\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{t_{x}}(d)}\n",
    "        {\\mathrm{d} d}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 & 1 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix}, \\\\\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{t_{y}}(d)}\n",
    "        {\\mathrm{d} d}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 1 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix}, \\\\\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{t_{z}}(d)}\n",
    "        {\\mathrm{d} d}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 1 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix},\n",
    " \\end{align*}\n",
    "\n",
    "for a prismatic joint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can make Python functions which perform these derivatives\n",
    "\n",
    "def dTRx(θ, flip):\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure rotation around\n",
    "    the x-axis by amount θ\n",
    "    '''\n",
    "    Rhx = np.array([\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, -1, 0],\n",
    "        [0, 1, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    # This is the T_R_x matrix in the maths above\n",
    "    Trx = smb.trotx(θ)\n",
    "\n",
    "    if flip:\n",
    "        return Rhx.T @ Trx.T\n",
    "    else:\n",
    "        return Rhx @ Trx\n",
    "\n",
    "def dTRy(θ, flip):\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure rotation around\n",
    "    the y-axis by amount θ\n",
    "    '''\n",
    "    Rhy = np.array([\n",
    "        [0, 0, 1, 0],\n",
    "        [0, 0, 0, 0],\n",
    "        [-1, 0, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    # This is the T_R_y matrix in the maths above\n",
    "    Try = smb.troty(θ)\n",
    "\n",
    "    if flip:\n",
    "        return Rhy.T @ Try.T\n",
    "    else:\n",
    "        return Rhy @ Try\n",
    "\n",
    "def dTRz(θ, flip):\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure rotation around\n",
    "    the z-axis by amount θ\n",
    "    '''\n",
    "    Rhz = np.array([\n",
    "        [0, -1, 0, 0],\n",
    "        [1, 0, 0, 0],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    # This is the T_R_z matrix in the maths above\n",
    "    Trz = smb.trotz(θ)\n",
    "\n",
    "    if flip:\n",
    "        return Rhz.T @ Trz.T\n",
    "    else:\n",
    "        return Rhz @ Trz\n",
    "\n",
    "def dTtx():\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure translation along\n",
    "    the x-axis by amount d\n",
    "    '''\n",
    "    Thx = np.array([\n",
    "        [0, 0, 0, 1],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    return Thx\n",
    "\n",
    "def dTty():\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure translation along\n",
    "    the y-axis by amount d\n",
    "    '''\n",
    "    Thy = np.array([\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 1],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    return Thy\n",
    "\n",
    "def dTtz():\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure translation along\n",
    "    the z-axis by amount d\n",
    "    '''\n",
    "    Thz = np.array([\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 1],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    return Thz"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we make a method which can take the derivative of an ET"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def dET(et, η):\n",
    "    '''\n",
    "    This method takes an ET and returns the derivative with respect to the joint coordinate\n",
    "    This is here for convenience\n",
    "    '''\n",
    "\n",
    "    if et.axis == 'Rx':\n",
    "        return dTRx(η, et.isflip)\n",
    "    elif et.axis == 'Ry':\n",
    "        return dTRy(η, et.isflip)\n",
    "    elif et.axis == 'Rz':\n",
    "        return dTRz(η, et.isflip)\n",
    "    elif et.axis == 'tx':\n",
    "        return dTtx()\n",
    "    elif et.axis == 'ty':\n",
    "        return dTty()\n",
    "    else:\n",
    "        return dTtz()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For the second derivative of an elementary transform with respect to the same joint variable, the result is\n",
    "%\n",
    "\\begin{align*}\n",
    "    \\dfrac{\\mathrm{d}^2 \\bf{T}_{R_{x}}(\\theta)}\n",
    "        {\\mathrm{d} \\theta^2}\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & -1 & 0 \\\\\n",
    "        0 & 1 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix}\n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & -1 & 0 \\\\\n",
    "        0 & 1 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix} \\bf{T}_{R_{x}}(\\theta),\\\\\n",
    "    \\dfrac{\\mathrm{d}^2 \\bf{T}_{R_{y}}(\\theta)}\n",
    "        {\\mathrm{d} \\theta^2}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 1 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        -1 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix}\n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 1 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        -1 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix} \\bf{T}_{R_{y}}(\\theta), \\\\\n",
    "    \\dfrac{\\mathrm{d}^2 \\bf{T}_{R_{z}}(\\theta)}\n",
    "        {\\mathrm{d} \\theta^2}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & -1 & 0 & 0 \\\\\n",
    "        1 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0  \n",
    "    \\end{pmatrix}\n",
    "    \\begin{pmatrix}\n",
    "        0 & -1 & 0 & 0 \\\\\n",
    "        1 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0  \n",
    "    \\end{pmatrix} \\bf{T}_{R_{z}}(\\theta),\n",
    "\\end{align*}\n",
    "\n",
    "for a revolute joint, or a zero matrix for a prismatic joint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can make Python functions which perform these derivatives\n",
    "\n",
    "def ddTRx(θ, flip):\n",
    "    '''\n",
    "    Calculates the second derivative of an SE3 which is a pure rotation around\n",
    "    the x-axis by amount θ\n",
    "    '''\n",
    "    Rhx = np.array([\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, -1, 0],\n",
    "        [0, 1, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    print(θ)\n",
    "\n",
    "    # This is the T_R_x matrix in the maths above\n",
    "    Trx = smb.trotx(θ)\n",
    "\n",
    "    if flip:\n",
    "        return Rhx @ Rhx @ Trx.T\n",
    "    else:\n",
    "        return Rhx @ Rhx @ Trx\n",
    "\n",
    "def ddTRy(θ, flip):\n",
    "    '''\n",
    "    Calculates the second derivative of an SE3 which is a pure rotation around\n",
    "    the y-axis by amount θ\n",
    "    '''\n",
    "    Rhy = np.array([\n",
    "        [0, 0, 1, 0],\n",
    "        [0, 0, 0, 0],\n",
    "        [-1, 0, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    # This is the T_R_y matrix in the maths above\n",
    "    Try = smb.troty(θ)\n",
    "\n",
    "    if flip:\n",
    "        return Rhy @ Rhy @ Try.T\n",
    "    else:\n",
    "        return Rhy @ Rhy @ Try\n",
    "\n",
    "def ddTRz(θ, flip):\n",
    "    '''\n",
    "    Calculates the second derivative of an SE3 which is a pure rotation around\n",
    "    the z-axis by amount θ\n",
    "    '''\n",
    "    Rhz = np.array([\n",
    "        [0, -1, 0, 0],\n",
    "        [1, 0, 0, 0],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    # This is the T_R_z matrix in the maths above\n",
    "    Trz = smb.trotz(θ)\n",
    "\n",
    "    if flip:\n",
    "        return Rhz @ Rhz @ Trz.T\n",
    "    else:\n",
    "        return Rhz @ Rhz @ Trz\n",
    "\n",
    "def ddTtx():\n",
    "    '''\n",
    "    Calculates the second derivative of an SE3 which is a pure translation along\n",
    "    the x-axis by amount d\n",
    "    '''\n",
    "    return np.zeros((4, 4))\n",
    "\n",
    "def ddTty():\n",
    "    '''\n",
    "    Calculates the second derivative of an SE3 which is a pure translation along\n",
    "    the y-axis by amount d\n",
    "    '''\n",
    "    return np.zeros((4, 4))\n",
    "\n",
    "def ddTtz():\n",
    "    '''\n",
    "    Calculates the second derivative of an SE3 which is a pure translation along\n",
    "    the z-axis by amount d\n",
    "    '''\n",
    "    return np.zeros((4, 4))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we make a method which can take the derivative of an ET"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ddET(et, η):\n",
    "    '''\n",
    "    This method takes an ET and returns the derivative with respect to the joint coordinate\n",
    "    This is here for convenience\n",
    "    '''\n",
    "\n",
    "    if et.axis == 'Rx':\n",
    "        return ddTRx(η, et.isflip)\n",
    "    elif et.axis == 'Ry':\n",
    "        return ddTRy(η, et.isflip)\n",
    "    elif et.axis == 'Rz':\n",
    "        return ddTRz(η, et.isflip)\n",
    "    elif et.axis == 'tx':\n",
    "        return ddTtx()\n",
    "    elif et.axis == 'ty':\n",
    "        return ddTty()\n",
    "    else:\n",
    "        return ddTtz()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We need the following utility methods (see Notebook 1 of Part I)\n",
    "def vex(mat):\n",
    "    '''\n",
    "    Converts a 3x3 skew symmetric matric to a 3 vector\n",
    "    '''\n",
    "\n",
    "    return np.array([mat[2, 1], mat[0, 2], mat[1, 0]])\n",
    "\n",
    "def ρ(tf):\n",
    "    '''\n",
    "    The method extracts the rotational component from an SE3\n",
    "    '''\n",
    "    return tf[:3, :3]\n",
    "\n",
    "def τ(tf):\n",
    "    '''\n",
    "    The method extracts the translation component from an SE3\n",
    "    '''\n",
    "    return tf[:3, 3]"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We also need the first derivative of an ETS (see Notebook 1 of Part I)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "def dT_j(ets, j, q):\n",
    "    '''\n",
    "    This methods calculates the dervative of an SE3 with respect to joint coordinate j\n",
    "    '''\n",
    "\n",
    "    # Allocate an identity matrix for the result\n",
    "    dT = np.eye(4)\n",
    "\n",
    "    # Find the jth variable et in the ets\n",
    "    et_j = ets.joints()[j]\n",
    "\n",
    "    # Loop overs the ETs in the robot ETS\n",
    "    for et in ets:\n",
    "        if et == et_j:\n",
    "            # This ET is variable and corresponds to joint j\n",
    "            dT = dT @ dET(et, q[et.jindex])\n",
    "        elif et.isjoint:\n",
    "            # This ET is a variable joint\n",
    "            # Use the q array to specify the joint angle for the variable ET\n",
    "            dT = dT @ et.A(q[et.jindex])\n",
    "        else:\n",
    "            # This ET is static\n",
    "            dT = dT @ et.A()\n",
    "\n",
    "    return dT"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Before going any further, lets remake the ETS panda model we made in Notebook 1 of Part I\n",
    "\n",
    "E1 = rtb.ET.tz(0.333) \n",
    "E2 = rtb.ET.Rz()\n",
    "E3 = rtb.ET.Ry() \n",
    "E4 = rtb.ET.tz(0.316) \n",
    "E5 = rtb.ET.Rz()\n",
    "E6 = rtb.ET.tx(0.0825) \n",
    "E7 = rtb.ET.Ry(flip=True) \n",
    "E8 = rtb.ET.tx(-0.0825) \n",
    "E9 = rtb.ET.tz(0.384) \n",
    "E10 = rtb.ET.Rz()\n",
    "E11 = rtb.ET.Ry(flip=True)\n",
    "E12 = rtb.ET.tx(0.088) \n",
    "E13 = rtb.ET.Rx(np.pi / 2) \n",
    "E14 = rtb.ET.tz(0.107)\n",
    "E15 = rtb.ET.Rz()\n",
    "panda = E1 * E2 * E3 * E4 * E5 * E6 * E7 * E8 * E9 * E10 * E11 * E12 * E13 * E14 * E15\n",
    "\n",
    "# And make a joint coordinate array q\n",
    "q = np.array([0, -0.3, 0, -2.2, 0, 2, 0.79])"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='hess'></a>\n",
    "### 1.2 The Manipulator Hessian\n",
    "---\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The second partial derivative of a pose with respect to the joint variables $q_j$ and $q_k$,\n",
    "can be obtained by taking the derivative of\n",
    "\n",
    "\\begin{align*}\n",
    "    \\frac{\\partial \\bf{T}(\\bf{q})}\n",
    "        {\\partial q_j} \n",
    "    &=\n",
    "    \\frac{\\partial} \n",
    "        {\\partial q_j}  \n",
    "    \\left(\n",
    "        \\bf{E}_1(\\eta_1) \\bf{E}_2(\\eta_2) \\cdots \\bf{E}_M(\\eta_M) \n",
    "    \\right)\\\\\n",
    "    &= \n",
    "    \\prod_{i=1}^{\\mu(j)-1} \\bf{E}_i(\\eta_i) \n",
    "    \\frac{\\mathrm{d} \\bf{E}_{\\mu(j)}(q_j)} \n",
    "        {\\mathrm{d} q_j} \n",
    "    \\prod_{i=\\mu(j)+1}^{M} \\bf{E}_i(\\eta_i).\n",
    "\\end{align*}\n",
    "\n",
    "with respect to $q_k$. This results in the following, where the function $\\mu(j)$ returns the index in the ETS where $q_j$ appears as a variable\n",
    "\n",
    "\\begin{align*}\n",
    "    \\frac{\\partial^2 \\bf{T}}\n",
    "        {\\partial q_k q_j} &= \n",
    "    \\begin{pmatrix} \n",
    "        \\bf{H}_{R_{j_k}} & \\bf{H}_{t_{j_k}} \\\\ \n",
    "        0  & 0 \n",
    "    \\end{pmatrix}  \\\\\n",
    "    &= \n",
    "    \\frac{\\partial} \n",
    "        {\\partial q_k} \n",
    "    \\left( \n",
    "    \\prod_{i=1}^{\\mu(j)-1} \\bf{E}_i(\\eta_i)\n",
    "    \\\n",
    "    \\frac{\\mathrm{d} \\bf{E}_{\\mu(j)}(q_j)} \n",
    "        {d q_j} \n",
    "    \\\n",
    "    \\prod_{i=\\mu(j)+1}^{M} \\bf{E}_i(\\eta_i)  \n",
    "    \\right)   \\\\\n",
    "    &= \\left\\{ \n",
    "    \\begin{matrix}\n",
    "        \\prod\\limits_{i=1}^{\\mu(k) - 1} \\bf{E}_i(\\eta_i)\n",
    "        \\\n",
    "        \\frac{\\mathrm{d} \\bf{E}_{\\mu(k)}(q_k)}\n",
    "            {d q_k}\n",
    "        \\\n",
    "        \\prod\\limits_{i=\\mu(k)+1}^{\\mu(j) - 1} \\bf{E}_i(\\eta_i) \n",
    "        \\\n",
    "        \\frac{\\mathrm{d} \\bf{E}_{\\mu(j)}(q_j)}\n",
    "            {d q_j} \n",
    "        \\\n",
    "        \\prod\\limits_{i=\\mu(j)+1}^{M} \\bf{E}_i(\\eta_i)\n",
    "        & \\text{if} \\ k < j \\\\\n",
    "        \\prod\\limits_{i=1}^{\\mu(k) - 1} E_i(\\eta_i) \n",
    "        \\\n",
    "        \\frac{\\mathrm{d}^2 \\bf{E}_{\\mu(k)}(q_k)}\n",
    "            {d q_k^2} \n",
    "        \\\n",
    "        \\prod\\limits_{i=\\mu(k)+1}^{M} E_i(\\eta_i) \n",
    "        & \\text{if} \\ k = j \\\\\n",
    "        \\prod\\limits_{i=1}^{\\mu(j) - 1} \\bf{E}_i(\\eta_i) \n",
    "        \\\n",
    "        \\frac{\\mathrm{d} \\bf{E}_{\\mu(j)}(q_j)}\n",
    "            {d q_j}\n",
    "        \\\n",
    "        \\prod\\limits_{i=\\mu(j)+1}^{\\mu(k) - 1} \\bf{E}_i(\\eta_i) \n",
    "        \\\n",
    "        \\frac{\\mathrm{d} \\bf{E}_{\\mu(k)}(q_k)}\n",
    "            {d q_k} \n",
    "        \\\n",
    "        \\prod\\limits_{i=\\mu(k)+1}^{M} \\bf{E}_i(\\eta_i)\n",
    "        & \\text{if} \\ k > j \\\\\n",
    "    \\end{matrix}\n",
    "    \\right.\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the above equation, we can fing the derivative of an elementary transform with respect to a joint coordinate using the method described in Section 1.1 above.\n",
    "\n",
    "For the second derivative of an elementary transform with respect to the same joint variable, as is the case for $k = j$ in the above equation, the result is a zero matrix."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we make a method which can take the second derivative of an a pose as described in the equation above."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ddT_jk(ets, j, k, q):\n",
    "    '''\n",
    "    This methods calculates the second dervative of an SE3 with\n",
    "    respect to joint coordinate j and k\n",
    "    '''\n",
    "\n",
    "    # Allocate an identity matrix for the result\n",
    "    ddT = np.eye(4)\n",
    "\n",
    "    # Find the jth variable et in the ets\n",
    "    et_j = ets.joints()[j]\n",
    "\n",
    "    # Find the kth variable et in the ets\n",
    "    et_k = ets.joints()[k]\n",
    "\n",
    "    # Loop overs the ETs in the robot ETS\n",
    "    for et in ets:\n",
    "\n",
    "        if et == et_j and j == k:\n",
    "            # Case 2 in the above equation\n",
    "            ddT = ddT @ ddET(et, q[et.jindex])\n",
    "        elif et == et_j:\n",
    "            # Case 1 or 3 in the above equation\n",
    "            # This ET is variable and corresponds to joint j\n",
    "            ddT = ddT @ dET(et, q[et.jindex])\n",
    "        elif et == et_k:\n",
    "            # Case 1 or 3 in the above equation\n",
    "            # This ET is variable and corresponds to joint j\n",
    "            ddT = ddT @ dET(et, q[et.jindex])\n",
    "        elif et.isjoint:\n",
    "            # This ET is a variable joint\n",
    "            # Use the q array to specify the joint angle for the variable ET\n",
    "            ddT = ddT @ et.A(q[et.jindex])\n",
    "        else:\n",
    "            # This ET is static\n",
    "            ddT = ddT @ et.A()\n",
    "\n",
    "    return ddT"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As for the manipulator Jacobian, to form the manipulator Hessian, we partition it into translational and rotational components.\n",
    "\n",
    "To form $\\bf{H}_{\\alpha_{jk}}$, the angular component of the manipulator Hessian of a joint variable $j$ with respect to another joint variable $k$, we take the partial derivative of the $j^{th}$ column of the manipulator Jacobian in \n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{J}_{\\omega_j}(\\bf{q}) \n",
    "    =\n",
    "    \\mathsf{V}_\\times\n",
    "    \\left(\n",
    "        \\rho\n",
    "        \\left(\n",
    "            \\frac{\\partial \\bf{T}(\\bf{q})}\n",
    "                {\\partial q_j} \n",
    "        \\right)\n",
    "        \\rho\n",
    "        \\left(\n",
    "            \\bf{T}(\\bf{q})\n",
    "        \\right)^\\top\n",
    "    \\right)\n",
    "\\end{align*}\n",
    "\n",
    "using the product rule\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{H}_{\\omega_{jk}}\n",
    "    &=\n",
    "    \\dfrac{\\partial \\bf{J}_{\\omega_j})(\\bf{q})}\n",
    "        {\\partial q_k} \\\\\n",
    "    &=\n",
    "    \\vee_\\times \\bigg(\n",
    "        \\rho\n",
    "        \\left(\n",
    "            \\frac{\\partial^2 \\bf{T}(\\bf{q})}\n",
    "                {\\partial q_j \\partial q_k} \n",
    "        \\right)\n",
    "        \\rho\n",
    "        \\left(\n",
    "            \\bf{T}(\\bf{q})\n",
    "        \\right)^\\top\n",
    "    \\bigg.\n",
    "    + \n",
    "    \\bigg.\n",
    "        \\rho\n",
    "        \\left(\n",
    "            \\frac{\\partial \\bf{T}(\\bf{q})}\n",
    "                {\\partial q_j} \n",
    "        \\right)\n",
    "        \\rho\n",
    "        \\left(\n",
    "            \\frac{\\partial \\bf{T}(\\bf{q})}\n",
    "                {\\partial q_k} \n",
    "        \\right)^\\top\n",
    "    \\bigg) \\\\\n",
    "    &=\n",
    "    \\mathsf{V}_\\times\n",
    "    \\left(\n",
    "        \\bf{H}_{R_{jk}} \\\n",
    "        \\rho\n",
    "        \\left(\n",
    "            \\bf{T}(\\bf{q})\n",
    "        \\right)^\\top\n",
    "        +\n",
    "        \\bf{J}_{R_j}\n",
    "        \\bf{J}_{R_k}^\\top\n",
    "    \\right)\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{H}_{\\omega_{jk}} \\in \\mathbb{R}^3$, and $\\bf{H}_{R_{jk}}$ is obtained from above.\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Hω_jk(ets, j, k, q):\n",
    "    '''\n",
    "    This method calculates the rotational component of the jth\n",
    "    slice, kth column of the manipulator Hessian\n",
    "    '''\n",
    "\n",
    "    # Calculate the forward kinematics at q\n",
    "    T = ets.eval(q)\n",
    "\n",
    "    # Calculate the second derivative of T with respect to q_j, q_k\n",
    "    ddT = ddT_jk(ets, j, k, q)\n",
    "\n",
    "    # Calculate the derivative of T with respect to q_j\n",
    "    dTj = dT_j(ets, j, q)\n",
    "\n",
    "    # Calculate the derivative of T with respect to q_k\n",
    "    dTk = dT_j(ets, k, q)\n",
    "\n",
    "    Hω = vex(ρ(ddT) @ (ρ(T).T) + ρ(dTj) @ (ρ(dTk).T))\n",
    "\n",
    "    return Hω"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[9.55336489e-01 5.96560643e-33 2.95520207e-01]\n"
     ]
    }
   ],
   "source": [
    "# Calculate the angular component of the third slice, second column of Panda manipulator Hessian\n",
    "print(Hω_jk(panda, 2, 1, q))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To form $\\bf{H}_{a_{jk}}$, the translational component of the manipulator Hessian for joint variable $j$ with respect to another joint variable $k$, we take the partial derivative of the $j^{th}$ translational component of the manipulator Jacobian in\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{J}_{v_{j}}(\\bf{q}) \n",
    "    &=\n",
    "    \\tau\n",
    "    \\left(\n",
    "        \\frac{\\partial \\bf{T}(\\bf{q})}\n",
    "            {\\partial q_j} \n",
    "    \\right)\n",
    "\\end{align*}\n",
    "\n",
    "which provides\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{H}_{v_{jk}}\n",
    "    &=\n",
    "    \\dfrac{\\partial \\bf{J}_{v_{j}}(\\bf{q})}\n",
    "        {\\partial q_k} \\\\\n",
    "    &=\n",
    "    \\tau\n",
    "    \\left(\n",
    "        \\frac{\\partial^2 \\bf{T}(\\bf{q})}\n",
    "            {\\partial q_j \\partial q_k} \n",
    "    \\right) \\\\\n",
    "    &= \n",
    "    \\bf{H}_{t_{jk}}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{H}_{a_{jk}} \\in \\mathbb{R}^3$, and $\\bf{H}_{t_{jk}}$ is obtained from above."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Hv_jk(ets, j, k, q):\n",
    "    '''\n",
    "    This method calculates the translation component of the jth\n",
    "    slice, kth column of the manipulator Hessian\n",
    "    '''\n",
    "\n",
    "    # Calculate the derivative of T with respect to q_j\n",
    "    ddT = ddT_jk(ets, j, k, q)\n",
    "\n",
    "    Hv = τ(ddT)\n",
    "\n",
    "    return Hv"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 0.03162066  0.         -0.102221  ]\n"
     ]
    }
   ],
   "source": [
    "# Calculate the translation component of the third slice, second column of Panda manipulator Hessian\n",
    "print(Hv_jk(panda, 2, 1, q))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Stacking the translational and angular components, we form the component of the manipulator Hessian for joint variable $j$ with respect to another joint variable $k$\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{H}_{jk}\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\bf{H}_{v_{jk}} \\\\\n",
    "        \\bf{H}_{\\omega_{jk}}\n",
    "    \\end{pmatrix}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{H}_{jk} \\in \\mathbb{R}^{6}$.\n",
    "\n",
    "The component of the manipulator Hessian for joint variable $j$ is formed by arranging the abpve into columns of a matrix\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{H}_j\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\bf{H}_{j1} &\n",
    "        \\cdots &\n",
    "        \\bf{H}_{jn}\n",
    "    \\end{pmatrix}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{H}_{j} \\in \\mathbb{R}^{6 \\times n}$.\n",
    "\n",
    "The whole manipulator Hessian is formed by arranging the above into _slices_ of a tensor\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{H}\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\bf{H}_1 &\n",
    "        \\cdots &\n",
    "        \\bf{H}_n\n",
    "    \\end{pmatrix}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{H} \\in \\mathbb{R}^{n \\times 6 \\times n}$ and last last two dimensions of $\\bf{H}$ define the dimension of the slices $\\bf{H}_i$.\n",
    "\n",
    "<br>\n",
    "\n",
    "<br>\n",
    "\n",
    "<img src=\"img/hessian_dark.png\" alt=\"drawing\" width=\"900\"/>\n",
    "\n",
    "_Visualisation of the Hessian $\\bf{H}(\\bf{q})$ representing a 7-joint manipulator. Each slice of the Hessian $\\bf{H}_i(\\bf{q})$ represents the acceleration of the end-effector caused by the velocities of each joint $\\bf{q}$ with respect to the velocity of joint $q_i$. Within a slice, the top three rows $\\bf{H}_{\\alpha_i}$ correspond to the linear acceleration, while the bottom three rows $\\bf{H}_{a_i}$ correspond to the angular acceleration, of the end-effector $\\bf{\\alpha}$ caused by the velocities of the joints._"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "# By using our previously defined methods, we can now calculate the manipulator Hessian\n",
    "\n",
    "def hessian0(ets, q):\n",
    "    '''\n",
    "    This method calculates the manipulator Hessian in the world frame\n",
    "    '''\n",
    "\n",
    "    # Allocate array for the Hessian\n",
    "    # It is a nx6xn matrix\n",
    "    H = np.zeros((ets.n, 6, ets.n))\n",
    "\n",
    "    for j in range(ets.n):\n",
    "        for k in range(ets.n):\n",
    "            Hv = Hv_jk(ets, j, k, q)\n",
    "            Hω = Hω_jk(ets, j, k, q)\n",
    "\n",
    "            H[k, :3, j] = Hv\n",
    "            H[k, 3:, j] = Hω\n",
    "\n",
    "    return H"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[[-0.46  0.   -0.53  0.    0.04  0.    0.  ]\n",
      "  [ 0.11  0.29  0.1   0.04 -0.03 -0.01  0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.   -1.   -0.    1.    0.    1.    1.  ]\n",
      "  [ 0.    0.   -0.3   0.    0.95  0.   -0.  ]\n",
      "  [-0.   -0.   -0.   -0.   -0.   -0.    0.  ]]\n",
      "\n",
      " [[ 0.   -0.46  0.03  0.48 -0.1   0.09  0.  ]\n",
      "  [ 0.29  0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.   -0.29 -0.1  -0.04  0.03  0.01  0.  ]\n",
      "  [ 0.    0.    0.96  0.   -0.32  0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]\n",
      "  [-0.    0.    0.3   0.   -0.95  0.    0.  ]]\n",
      "\n",
      " [[-0.53  0.03 -0.5   0.    0.03  0.    0.  ]\n",
      "  [ 0.1   0.    0.11  0.18 -0.06  0.02  0.  ]\n",
      "  [ 0.   -0.1  -0.16  0.    0.01  0.    0.  ]\n",
      "  [ 0.    0.   -0.    0.96  0.    0.96  0.96]\n",
      "  [ 0.    0.    0.   -0.    0.81 -0.    0.  ]\n",
      "  [-0.    0.   -0.    0.3   0.    0.3   0.3 ]]\n",
      "\n",
      " [[ 0.    0.48  0.   -0.48  0.1  -0.09  0.  ]\n",
      "  [ 0.04  0.    0.18  0.    0.    0.    0.  ]\n",
      "  [ 0.   -0.04  0.    0.04 -0.03 -0.01  0.  ]\n",
      "  [ 0.    0.    0.    0.    0.32  0.   -0.  ]\n",
      "  [-0.    0.   -0.   -0.   -0.   -0.    0.  ]\n",
      "  [-0.    0.   -0.    0.    0.95  0.   -0.  ]]\n",
      "\n",
      " [[ 0.04 -0.1   0.03  0.1  -0.01  0.    0.  ]\n",
      "  [-0.03  0.   -0.06  0.    0.11 -0.08  0.  ]\n",
      "  [ 0.    0.03  0.01 -0.03 -0.03  0.    0.  ]\n",
      "  [ 0.    0.   -0.   -0.   -0.   -0.32 -0.32]\n",
      "  [ 0.    0.    0.    0.   -0.    0.   -0.  ]\n",
      "  [-0.    0.    0.    0.   -0.   -0.95 -0.95]]\n",
      "\n",
      " [[ 0.    0.09  0.   -0.09  0.   -0.09  0.  ]\n",
      "  [-0.01  0.    0.02  0.   -0.08  0.    0.  ]\n",
      "  [ 0.    0.01  0.   -0.01  0.   -0.01  0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]\n",
      "  [ 0.   -0.   -0.   -0.   -0.   -0.    0.  ]\n",
      "  [-0.    0.   -0.    0.    0.    0.   -0.  ]]\n",
      "\n",
      " [[ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.   -0.   -0.    0.    0.    0.  ]\n",
      "  [-0.   -0.   -0.   -0.   -0.   -0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]]]\n"
     ]
    }
   ],
   "source": [
    "# Calculate the manipulator Hessian of the Panda in the world frame\n",
    "H = hessian0(panda, q)\n",
    "\n",
    "print(np.round(H, 2))\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "   \n",
    "<a id='fhess'></a>\n",
    "### 1.3 Fast Manipulator Hessian\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Calculatint the manipulator Hessian using the above has $\\mathcal{O}(n^3)$ time complexity which we can improve.\n",
    "\n",
    "We revisit the previous equations while substituting in\n",
    "\n",
    "\\begin{align*}\n",
    "    \\frac{\\mathrm{d}\\bf{R}(\\theta)}\n",
    "        {\\mathrm{d} \\theta} \n",
    "    &= \n",
    "    [\n",
    "        \\hat{\\bf{\\omega}}\n",
    "    ]_\\times\n",
    "    \\bf{R}(\\theta(t)) \n",
    "\\end{align*}\n",
    "\n",
    "and simplify\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{H}_{\\omega_{jk}}\n",
    "    &=\n",
    "    \\vee_\\times \\bigg(\n",
    "        {[\\hat{\\bf{\\omega}}_k]_\\times}\n",
    "        {[\\hat{\\bf{\\omega}}_j]_\\times}\n",
    "        \\bf{R}(\\bf{q})\n",
    "        \\bf{R}(\\bf{q})^\\top + \n",
    "    \\bigg. \\\\\n",
    "    & \\qquad \\\n",
    "    \\bigg.\n",
    "        {[\\hat{\\bf{\\omega}}_j]_\\times}\n",
    "        \\bf{R}(\\bf{q})\n",
    "        \\left(\n",
    "        {[\\hat{\\bf{\\omega}}_k]_\\times}\n",
    "        \\bf{R}(\\bf{q})\n",
    "        \\right)^\\top\n",
    "    \\bigg) \\\\\n",
    "    &=\n",
    "    \\vee_\\times \\bigg(\n",
    "        {[\\hat{\\bf{\\omega}}_k]_\\times}\n",
    "        {[\\hat{\\bf{\\omega}}_j]_\\times} + \n",
    "        {[\\hat{\\bf{\\omega}}_j]_\\times}\n",
    "        \\bf{R}(\\bf{q}) \\bf{R}(\\bf{q})^\\top\n",
    "        {[\\hat{\\bf{\\omega}}_k]_\\times}^\\top\n",
    "    \\bigg) \\\\\n",
    "    &=\n",
    "    \\vee_\\times \\bigg(\n",
    "        {[\\hat{\\bf{\\omega}}_k]_\\times}\n",
    "        {[\\hat{\\bf{\\omega}}_j]_\\times} - \n",
    "        {[\\hat{\\bf{\\omega}}_j]_\\times}\n",
    "        {[\\hat{\\bf{\\omega}}_k]_\\times}\n",
    "    \\bigg)\n",
    "\\end{align*}\n",
    "\n",
    "Since we know that $\\bf{J}_{\\omega_x} = {[\\hat{\\bf{\\omega}}_x}]_\\times$, and using the identity $[\\bf{a} \\times \\bf{b}]_\\times = [\\bf{a}]_\\times[\\bf{b}]_\\times-[\\bf{b}]_\\times[\\bf{a}]_\\times$ we show that\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{H}_{\\omega_{jk}}\n",
    "    &=\n",
    "    \\vee_\\times \\bigg(\n",
    "        [\\bf{J}_{\\omega_{k}}]_\\times\n",
    "        [\\bf{J}_{\\omega_{j}}]_\\times -\n",
    "        [\\bf{J}_{\\omega_{j}}]_\\times\n",
    "        [\\bf{J}_{\\omega_{k}}]_\\times\n",
    "    \\bigg)\\\\\n",
    "    &= \\bf{J}_{\\omega_{k}} \\times \\bf{J}_{\\omega_{j}}\n",
    "\\end{align*}\n",
    "\n",
    "which means that the rotational component of the manipulator Hessian can be calculated from the rotational components of the manipulator Jacobian. \n",
    "A key relationship is that the velocity of joint $j$, with respect to the velocity of the same, or preceding joint $k$, does not contribute acceleration to the end-effector from the perspective of joint $j$. Consequently, $\\bf{H}_{\\omega_{jk}}=0$ when $k\\geq j$.\n",
    "\n",
    "For the translational component of the manipulator Hessian $\\bf{H}_v$, we can see in (\\ref{eq:ddets}) that two of the conditions will have the same result: when $k < j$, and when $k > j$. Therefore, we have\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{H}_{\\omega_{jk}}(\\bf{q}) = \\bf{H}_{v_{kj}}(\\bf{q})\n",
    "\\end{align*}\n",
    "\n",
    "and by exploiting this relationship, we can simplify (\\ref{eq:hess1}) to\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{H}_{\\omega_{jk}}(\\bf{q})\n",
    "    &= [\\bf{J}_{\\omega_{k}}]\\times \\bf{J}_{v_j} \\\\\n",
    "    &= \\bf{J}_{\\omega_a} \\times \\bf{J}_{v_b}\n",
    "\\end{align*}\n",
    "\n",
    "where $a = \\min(j,k)$, and $b = \\max(j,k)$. This means that the translational component of the manipulator Hessian can be calculated from components of the manipulator Jacobian.\n",
    "\n",
    "Through this simplification, computation of the manipulator Hessian reduces to $\\mathcal{O}(n^2)$ time complexity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Now making the efficient version in Python\n",
    "\n",
    "def hessian0_efficient(ets, q):\n",
    "    '''\n",
    "    This method calculates the manipulator Hessian in the world frame using a more efficient method\n",
    "    '''\n",
    "\n",
    "    # Make the Jacobian\n",
    "    J = ets.jacob0(q)\n",
    "\n",
    "    # Allocate space for the Hessian\n",
    "    H = np.zeros((ets.n, 6, ets.n))\n",
    "\n",
    "    for j in range(ets.n):\n",
    "        for i in range(j, ets.n):\n",
    "\n",
    "            H[j, :3, i] = np.cross(J[3:, j], J[:3, i])\n",
    "            H[j, 3:, i] = np.cross(J[3:, j], J[3:, i])\n",
    "\n",
    "            if i != j:\n",
    "                H[i, :3, j] = H[j, :3, i]\n",
    "\n",
    "    return H"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The manipulator Hessian (world-frame) \n",
      "in configuration q = [ 0.   -0.3   0.   -2.2   0.    2.    0.79] \n",
      "is: \n",
      "[[[-0.46 -0.   -0.53  0.    0.04  0.    0.  ]\n",
      "  [ 0.11  0.29  0.1   0.04 -0.03 -0.01  0.  ]\n",
      "  [ 0.    0.    0.    0.   -0.    0.    0.  ]\n",
      "  [ 0.   -1.    0.    1.   -0.    1.    1.  ]\n",
      "  [ 0.   -0.   -0.3   0.    0.95  0.   -0.  ]\n",
      "  [ 0.    0.    0.   -0.    0.   -0.   -0.  ]]\n",
      "\n",
      " [[-0.   -0.46  0.03  0.48 -0.1   0.09  0.  ]\n",
      "  [ 0.29 -0.    0.    0.   -0.    0.    0.  ]\n",
      "  [ 0.   -0.29 -0.1  -0.04  0.03  0.01 -0.  ]\n",
      "  [ 0.    0.    0.96  0.   -0.32  0.    0.  ]\n",
      "  [ 0.    0.    0.    0.   -0.    0.    0.  ]\n",
      "  [ 0.    0.    0.3   0.   -0.95  0.    0.  ]]\n",
      "\n",
      " [[-0.53  0.03 -0.5   0.    0.03  0.    0.  ]\n",
      "  [ 0.1   0.    0.11  0.18 -0.06  0.02  0.  ]\n",
      "  [ 0.   -0.1  -0.16 -0.    0.01  0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.96 -0.    0.96  0.96]\n",
      "  [ 0.    0.    0.    0.    0.81  0.    0.  ]\n",
      "  [ 0.    0.    0.    0.3  -0.    0.3   0.3 ]]\n",
      "\n",
      " [[ 0.    0.48  0.   -0.48  0.1  -0.09 -0.  ]\n",
      "  [ 0.04  0.    0.18 -0.    0.   -0.    0.  ]\n",
      "  [ 0.   -0.04 -0.    0.04 -0.03 -0.01  0.  ]\n",
      "  [ 0.    0.    0.    0.    0.32  0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.95  0.   -0.  ]]\n",
      "\n",
      " [[ 0.04 -0.1   0.03  0.1  -0.01  0.    0.  ]\n",
      "  [-0.03 -0.   -0.06  0.    0.11 -0.08 -0.  ]\n",
      "  [-0.    0.03  0.01 -0.03 -0.03  0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.   -0.32 -0.32]\n",
      "  [ 0.    0.    0.    0.    0.   -0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.   -0.95 -0.95]]\n",
      "\n",
      " [[ 0.    0.09  0.   -0.09  0.   -0.09 -0.  ]\n",
      "  [-0.01  0.    0.02 -0.   -0.08 -0.    0.  ]\n",
      "  [ 0.    0.01  0.   -0.01  0.   -0.01  0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]]\n",
      "\n",
      " [[ 0.    0.    0.   -0.    0.   -0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.   -0.    0.    0.  ]\n",
      "  [ 0.   -0.   -0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]]]\n"
     ]
    }
   ],
   "source": [
    "H = hessian0_efficient(panda, q)\n",
    "\n",
    "print(f\"The manipulator Hessian (world-frame) \\nin configuration q = {q} \\nis: \\n{np.round(H, 2)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='frame'></a>\n",
    "### 1.4 Changing Hessian Frame\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We have been calculating the manipulator Hessian in the world-frame\n",
    "\n",
    "\\begin{equation*}\n",
    "     {^0\\bf{H}}(\\bf{q}).\n",
    "\\end{equation*}\n",
    "\n",
    "is the manipulator Hessian matrix expressed in the world-coordinate frame.\n",
    "\n",
    "The Hessian expressed in the end-effector frame is\n",
    "\n",
    "\\begin{equation*}\n",
    "    {^e\\bf{H}}(\\bf{q}) =\n",
    "    \\begin{pmatrix} \n",
    "        {{^0\\bf{R}}_e^\\top} & \\bf{0} \\\\ \n",
    "        \\bf{0} & {{^0\\bf{R}}_e^\\top}\n",
    "     \\end{pmatrix}\n",
    "     {^0\\bf{H}}(\\bf{q})\n",
    "\\end{equation*}\n",
    "\n",
    "where ${^0\\bf{R}}_e$ is a rotation matrix representing the orientation of the end-effector in the world frame. And we perform the matrix multiplication along slices of the tensor ${^0\\bf{H}}(\\bf{q})$.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Now making it in Python\n",
    "\n",
    "def hessiane(ets, q):\n",
    "    '''\n",
    "    This method calculates the manipulator Hessian in the end-effector frame\n",
    "    '''\n",
    "\n",
    "    # Allocate the return array\n",
    "    He = np.zeros((ets.n, 6, ets.n))\n",
    "\n",
    "    # Calculate world-frame Hessian\n",
    "    H0 = hessian0_efficient(ets, q)\n",
    "\n",
    "    # Calculate the forward kineamtics\n",
    "    T = ets.eval(q)\n",
    "\n",
    "    # Extract rotation from T and transpose\n",
    "    R = T[:3, :3].T\n",
    "\n",
    "    # Make our velocity transform matrix\n",
    "    tr = np.zeros((6, 6))\n",
    "\n",
    "    # Put the rotation in the top left and bottom right of the matrix\n",
    "    tr[:3, :3] = R\n",
    "    tr[3:, 3:] = R\n",
    "\n",
    "    # Perform the matrix multiplication to calculate the Hessian\n",
    "    # in the end-effector frame\n",
    "    for i in range(ets.n):\n",
    "        He[i] = tr @ H0[i]\n",
    "\n",
    "    return He\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The manipulator Hessian (end-effector) \n",
      "in configuration q = [ 0.   -0.3   0.   -2.2   0.    2.    0.79] \n",
      "is: \n",
      "[[[-0.29  0.   -0.33  0.    0.02 -0.    0.  ]\n",
      "  [ 0.36  0.    0.41  0.   -0.03 -0.    0.  ]\n",
      "  [-0.11 -0.29 -0.1  -0.04  0.03  0.01  0.  ]\n",
      "  [ 0.   -0.63 -0.    0.63  0.    0.63  0.63]\n",
      "  [ 0.    0.78 -0.   -0.78  0.   -0.78 -0.78]\n",
      "  [ 0.    0.    0.3  -0.   -0.95 -0.    0.  ]]\n",
      "\n",
      " [[ 0.   -0.52 -0.06  0.27 -0.04  0.06  0.  ]\n",
      "  [ 0.    0.18 -0.09 -0.39  0.1  -0.06  0.  ]\n",
      "  [-0.29 -0.   -0.   -0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.83  0.   -0.94  0.    0.  ]\n",
      "  [ 0.    0.   -0.56  0.   -0.34  0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.   -0.    0.    0.  ]]\n",
      "\n",
      " [[-0.33 -0.06 -0.44  0.    0.03  0.    0.  ]\n",
      "  [ 0.41 -0.09  0.29  0.   -0.02  0.    0.  ]\n",
      "  [-0.1  -0.   -0.11 -0.18  0.06 -0.02  0.  ]\n",
      "  [ 0.    0.    0.    0.83  0.    0.83  0.83]\n",
      "  [ 0.    0.    0.   -0.56  0.   -0.56 -0.56]\n",
      "  [ 0.    0.    0.    0.   -0.81  0.   -0.  ]]\n",
      "\n",
      " [[ 0.    0.27  0.   -0.27  0.04 -0.06  0.  ]\n",
      "  [ 0.   -0.39  0.    0.39 -0.1   0.06  0.  ]\n",
      "  [-0.04 -0.   -0.18  0.   -0.   -0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.94  0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.34  0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]]\n",
      "\n",
      " [[ 0.02 -0.04  0.03  0.04 -0.03 -0.    0.  ]\n",
      "  [-0.03  0.1  -0.02 -0.1  -0.01 -0.    0.  ]\n",
      "  [ 0.03  0.    0.06 -0.   -0.11  0.08  0.  ]\n",
      "  [ 0.    0.    0.    0.    0.   -0.94 -0.94]\n",
      "  [ 0.    0.    0.    0.    0.   -0.34 -0.34]\n",
      "  [ 0.    0.    0.    0.    0.   -0.   -0.  ]]\n",
      "\n",
      " [[-0.    0.06  0.   -0.06 -0.   -0.06  0.  ]\n",
      "  [-0.   -0.06  0.    0.06 -0.    0.06  0.  ]\n",
      "  [ 0.01  0.   -0.02 -0.    0.08 -0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]]\n",
      "\n",
      " [[ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]]]\n"
     ]
    }
   ],
   "source": [
    "# Calculate the manipulator Hessian of the Panda in the end-effector frame\n",
    "H = hessiane(panda, q)\n",
    "\n",
    "print(f\"The manipulator Hessian (end-effector) \\nin configuration q = {q} \\nis: \\n{np.round(H, 2)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='rtb'></a>\n",
    "### 1.5 Robotics Toolbox Hessians\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The `ETS` class within the Robotics Toolbox supports many kinematic methods (such as the previously discussed `.eval` and `.fkine`, `jacob0`, and `jacobe` methods).\n",
    "\n",
    "The `.hessian0` and `.hessiane` methods in the `ETS` class calculate the world and end-effector frame Hessians respectively.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The manipulator Hessian (world frame) is: \n",
      "[[[-0.46 -0.   -0.53  0.    0.04  0.    0.  ]\n",
      "  [ 0.11  0.29  0.1   0.04 -0.03 -0.01  0.  ]\n",
      "  [ 0.    0.    0.    0.   -0.    0.    0.  ]\n",
      "  [ 0.   -1.    0.    1.   -0.    1.    1.  ]\n",
      "  [ 0.   -0.   -0.3   0.    0.95  0.   -0.  ]\n",
      "  [ 0.    0.    0.   -0.    0.   -0.   -0.  ]]\n",
      "\n",
      " [[-0.   -0.46  0.03  0.48 -0.1   0.09  0.  ]\n",
      "  [ 0.29 -0.    0.    0.   -0.    0.    0.  ]\n",
      "  [ 0.   -0.29 -0.1  -0.04  0.03  0.01 -0.  ]\n",
      "  [ 0.    0.    0.96  0.   -0.32  0.    0.  ]\n",
      "  [ 0.    0.    0.    0.   -0.    0.    0.  ]\n",
      "  [ 0.    0.    0.3   0.   -0.95  0.    0.  ]]\n",
      "\n",
      " [[-0.53  0.03 -0.5   0.    0.03  0.    0.  ]\n",
      "  [ 0.1   0.    0.11  0.18 -0.06  0.02  0.  ]\n",
      "  [ 0.   -0.1  -0.16 -0.    0.01  0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.96 -0.    0.96  0.96]\n",
      "  [ 0.    0.    0.    0.    0.81  0.    0.  ]\n",
      "  [ 0.    0.    0.    0.3  -0.    0.3   0.3 ]]\n",
      "\n",
      " [[ 0.    0.48  0.   -0.48  0.1  -0.09 -0.  ]\n",
      "  [ 0.04  0.    0.18 -0.    0.   -0.    0.  ]\n",
      "  [ 0.   -0.04 -0.    0.04 -0.03 -0.01  0.  ]\n",
      "  [ 0.    0.    0.    0.    0.32  0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.95  0.   -0.  ]]\n",
      "\n",
      " [[ 0.04 -0.1   0.03  0.1  -0.01  0.    0.  ]\n",
      "  [-0.03 -0.   -0.06  0.    0.11 -0.08 -0.  ]\n",
      "  [-0.    0.03  0.01 -0.03 -0.03  0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.   -0.32 -0.32]\n",
      "  [ 0.    0.    0.    0.   -0.   -0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.   -0.95 -0.95]]\n",
      "\n",
      " [[ 0.    0.09  0.   -0.09  0.   -0.09 -0.  ]\n",
      "  [-0.01  0.    0.02 -0.   -0.08 -0.    0.  ]\n",
      "  [ 0.    0.01  0.   -0.01  0.   -0.01  0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]]\n",
      "\n",
      " [[ 0.    0.    0.   -0.    0.   -0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.   -0.    0.    0.  ]\n",
      "  [ 0.   -0.   -0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]]]\n",
      "\n",
      "The manipulator Hessian (end-effector frame) is: \n",
      "[[[-0.29  0.   -0.33  0.    0.02 -0.    0.  ]\n",
      "  [ 0.36  0.    0.41  0.   -0.03 -0.    0.  ]\n",
      "  [-0.11 -0.29 -0.1  -0.04  0.03  0.01  0.  ]\n",
      "  [ 0.   -0.63 -0.    0.63  0.    0.63  0.63]\n",
      "  [ 0.    0.78 -0.   -0.78  0.   -0.78 -0.78]\n",
      "  [-0.    0.    0.3  -0.   -0.95 -0.    0.  ]]\n",
      "\n",
      " [[ 0.   -0.52 -0.06  0.27 -0.04  0.06  0.  ]\n",
      "  [ 0.    0.18 -0.09 -0.39  0.1  -0.06 -0.  ]\n",
      "  [-0.29 -0.   -0.   -0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.83  0.   -0.94  0.    0.  ]\n",
      "  [ 0.    0.   -0.56  0.   -0.34  0.   -0.  ]\n",
      "  [ 0.   -0.    0.    0.   -0.    0.    0.  ]]\n",
      "\n",
      " [[-0.33 -0.06 -0.44  0.    0.03  0.    0.  ]\n",
      "  [ 0.41 -0.09  0.29  0.   -0.02  0.    0.  ]\n",
      "  [-0.1  -0.   -0.11 -0.18  0.06 -0.02  0.  ]\n",
      "  [ 0.    0.   -0.    0.83  0.    0.83  0.83]\n",
      "  [ 0.    0.    0.   -0.56  0.   -0.56 -0.56]\n",
      "  [ 0.    0.   -0.    0.   -0.81  0.    0.  ]]\n",
      "\n",
      " [[ 0.    0.27  0.   -0.27  0.04 -0.06 -0.  ]\n",
      "  [ 0.   -0.39  0.    0.39 -0.1   0.06  0.  ]\n",
      "  [-0.04 -0.   -0.18  0.   -0.   -0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.94  0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.34  0.    0.  ]\n",
      "  [ 0.    0.    0.   -0.    0.   -0.    0.  ]]\n",
      "\n",
      " [[ 0.02 -0.04  0.03  0.04 -0.03 -0.    0.  ]\n",
      "  [-0.03  0.1  -0.02 -0.1  -0.01 -0.   -0.  ]\n",
      "  [ 0.03  0.    0.06 -0.   -0.11  0.08  0.  ]\n",
      "  [ 0.    0.    0.    0.    0.   -0.94 -0.94]\n",
      "  [ 0.    0.    0.    0.   -0.   -0.34 -0.34]\n",
      "  [ 0.    0.    0.    0.   -0.   -0.    0.  ]]\n",
      "\n",
      " [[-0.    0.06  0.   -0.06 -0.   -0.06 -0.  ]\n",
      "  [-0.   -0.06  0.    0.06 -0.    0.06  0.  ]\n",
      "  [ 0.01  0.   -0.02 -0.    0.08 -0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.   -0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.   -0.    0.  ]]\n",
      "\n",
      " [[ 0.    0.    0.   -0.    0.   -0.    0.  ]\n",
      "  [ 0.   -0.    0.    0.   -0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]\n",
      "  [ 0.    0.    0.    0.    0.    0.    0.  ]]]\n"
     ]
    }
   ],
   "source": [
    "# Note: The panda object which we have been using is an instance of the ETS class\n",
    "\n",
    "# Calculate the world frame Hessian using the ETS class\n",
    "H0 = panda.hessian0(q)\n",
    "\n",
    "# Calculate the end-effector frame Hessian using the ETS class\n",
    "He = panda.hessiane(q)\n",
    "\n",
    "# View our Hessians\n",
    "print(f\"The manipulator Hessian (world frame) is: \\n{np.round(H0, 2)}\")\n",
    "print(f\"\\nThe manipulator Hessian (end-effector frame) is: \\n{np.round(He, 2)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='fast'></a>\n",
    "### 1.6 Speed Comparison\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this section we will briefly cover a speed comparison between the two `hessian0` methods shown here and the Hessian implementation provided by the Robotics Toolbox.\n",
    "\n",
    "Kinematic methods in the toolbox are written using C extensions and wrapped by python methods. These C methods also have Python fallbacks to rely on if something goes wrong.\n",
    "\n",
    "_This takes around 30 seconds on a decent desktop computer_"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Timing to Calculate Manipulator Hessian over 1000 iterations\n",
      "\n",
      "┌───────────────────────┬─────────────────┬───────────────────┐\n",
      "│              Function │ Total Time (ms) │ Average Time (us) │\n",
      "├───────────────────────┼─────────────────┼───────────────────┤\n",
      "│          our hessian0\u001b[0m │      15823.0887\u001b[0m │        15823.0887\u001b[0m │\n",
      "│our hessian0_efficient\u001b[0m │        638.9757\u001b[0m │          638.9757\u001b[0m │\n",
      "│      toolbox hessian0\u001b[0m │           1.007\u001b[0m │             1.007\u001b[0m │\n",
      "└───────────────────────┴─────────────────┴───────────────────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Number of iterations to test over\n",
    "n = 1000\n",
    "\n",
    "# Make a random matrix of q values\n",
    "qt = np.random.random((n, 7))\n",
    "\n",
    "# Slow Hessian implementation\n",
    "start = timer()\n",
    "for q in qt:\n",
    "    hessian0(panda, q)\n",
    "slow_time = timer() - start\n",
    "\n",
    "# Improved Hessian implementation\n",
    "start = timer()\n",
    "for q in qt:\n",
    "    hessian0_efficient(panda, q)\n",
    "improved_time = timer() - start\n",
    "\n",
    "# Toolbox C improved Hessian implementation\n",
    "start = timer()\n",
    "for q in qt:\n",
    "    panda.hessian0(q)\n",
    "rtb_time = timer() - start\n",
    "\n",
    "# Make a table\n",
    "print(f\"\\nTiming to Calculate Manipulator Hessian over {n} iterations\\n\")\n",
    "table = ANSITable(\n",
    "    \"Function\",\n",
    "    \"Total Time (ms)\",\n",
    "    \"Average Time (us)\",\n",
    "    border=\"thin\",\n",
    ")\n",
    "\n",
    "table.row(\n",
    "    \"our hessian0\",\n",
    "    np.round(slow_time * 1000.0, 4),\n",
    "    np.round((slow_time / n) * 1000000.0, 4),\n",
    ")\n",
    "\n",
    "table.row(\n",
    "    \"our hessian0_efficient\",\n",
    "    np.round(improved_time * 1000.0, 4),\n",
    "    np.round((improved_time / n) * 1000000.0, 4),\n",
    ")\n",
    "\n",
    "table.row(\n",
    "    \"toolbox hessian0\",\n",
    "    np.round(rtb_time * 1000.0, 4),\n",
    "    np.round((rtb_time / n) * 1000000.0, 4),\n",
    ")\n",
    "table.print()"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937"
  },
  "kernelspec": {
   "display_name": "Python 3.8.10 ('rtb')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.0"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
